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Abstract 

This paper presents an efficient method of simulating the motion of space based manipulators. Since the 
manipulators will come into contact with different objects in their environment while carrying out different 
task, an important part of the simulation is the modeling of those contacts. An inverse dynamics controller is 
used to control a two armed manipulator whose task is to grasp an object floating in space. Simulation results 
are presented and an evaluation is made the performance of the controller. 


1 Introduction 

Robotic manipulators carried by future spacecraft are expected to perform many important tasks in space. 
This paper presents a methodology for the simulation and control of these robots. The main idea is the need 
to include not only the dynamics of the robot in the simulation, but also the dynamics of the objects in which 
the robot comes into contact. The same is true in the design of the controller. If the robot is to be used in 
grappling a satellite, then the controller and the simulation must include a model of the satellite dynamics. 

The model we use is called the dynamic world model as it includes the dynamics of the objects in the 
robots environment and the dynamics of the robot itself. The next section presents the formulation of this 
world model. Included in the model is the dynamics of contact. A recently reported distance algorithm is 
used to detect contact [4]. 

The next section deals with the control of the robot. Feedback functions can be utilized by the controller 
either at the joint level or at the Cartesian level. An example is provided for an implementation of inverse 
dynamics control. 

The next section presents an example simulation. In this simulation, two PUMA 560 manipulators are 
mounted on a single base. The task is to grapple an object floating in space. The inverse dynamics control 
algorithm which has been developed for terrestrial-based robots is used for control. Compliance is obtained 
by using a relatively small position error gains in the controller. 

The final section concludes the paper with a discussion of the merits and limitations of the method. 


2 Dynamic World Model 

This section presents the world model used by the simulation. First, the data structure which is used to 
store the model is presented. Next, the kinematic modeling of the system is presented. The systems dynamic 
equations of motion are then presented followed by the dynamics of contact. 

1 Tliis work was supported in part by a grant from the NASA sponsored Center for Autonomous and Man- Controlled Robotic 
and Sensing Systems, CAMRSS, at ER.IM, Ann Arbor, MI 
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Figure 1: An Example System 


2.1 The Data Structure 

In the implementation of the controller, several quantities associated with each link are needed, such as: the 
link velocity, acceleration, and inertial forces and moments. This section presents the data structure used to 
store this information. The form of this data structure is important since the design of the control algorithm is 
directly linked to this data structure. An example system is illustrated in Figure 1(a) as a basis for discussion. 

In general, the number of rigid links in the system is m-|- 1. For the example in Figure 1(a), m-f 1 = 8. The 
inertial link is a imaginary link fixed with respect to an inertial reference frame and numbered 0. All other 
links are numbered in an arbitrary order from 1 tom. It is apparent that a graph data structure could be used 
to store the information. Each record or item in the data structure would be associated with a particular link 
and all of the information needed about that link would be stored in the associated data record. The problem 
with using this type of data structure is that it does not lead to a particularly simple or efficient algorithm 
for the controller. A much better data structure is a binary tree data structure. The remainder of this section 
develops the method of obtaining this type of data structure. 

We begin by selecting a set of joints such that if the associated links were disconnected at these joints 
there would be a unique sequence of links connecting any given link to the inertial link. An example of two 
joints which accomplish this are indicated in Figure 1(a) by the large arrows. The resulting structure is a 
tree structure and we can now establish relationships between different links in terms of their descendants and 
predecessors. For example, the descendants of link 1 are links 2 and 3. The predecessors of link 7 are links 0 
and 6. The immediate descendant of link 1 is link 2 and the immediate predecessor of link 2 is link 1. We also 
note that some links may have more than one immediate descendant. For example, link 0 has three: 1, 6, and 
4. The fact that a link could have an arbitrary number of immediate descendants causes practical problems 
in the implementation of the controller. To get around this problem we introduce the concept of connector 
links. 

An example of a link with four joints is illustrated in Figure 2. The joint connecting the immediate 
predecessor to the link is assigned the same number as the link. One of the other three joints is used to locate 
the link coordinates. This determines the D~H kinematic parameters a,-, a*, d t -, and 0, as illustrated in the 
figure. To locate the remaining two joints with respect to link i coordinates we insert fictitious links, j and 
it, called connector links. First is link j which is located relative to link i coordinates. The D-H kinematic 
parameters [1], a,*, a ; , dj y and 0 ; are used to locate this link j coordinates with respect to link i coordinates. 
Next is link k which is located relative to link j coordinates. Again the D-II kinematic parameters, a*, or*, 
d*, and 0 * are used to locate this link k coordinates with respect to link j coordinates. Note that all of 
the kinematic parameters, a, a d, and 0, associated with connector links are constant and that for both the 
connector and successor links the position and orientation of the descendant with respect to the current link 
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Figure 2: An Example Link, Numbered i 


is described using homogeneous transforms parameterized by the D-H kinematic parameters a, or, d and 0. 
The real links are called successor links to distinguish them from the imaginary connector links. 

Therefore, each link can only have to two immediate descendants. One would be a connector link and 
the other would be a successor link. Only one of each type is allowed. Hence, with the introduction of the 
connector links we have converted a general tree data structure into a binary tree data structure. 

For a given link, the first joint encountered when moving in the direction of the inertial link is numbered 
the same as the given link. Thus, all of the joints except those where the kinematic loops have been broken 
have been assigned a number. There are m of these. Assuming there are r independent loops the remaining 
r joints located at the points where the kinematic loops have been broken are numbered from m + 1 to m + r 
making a total of m + r joints. Finally, a fictitious successor link called a terminating link is associated with 
these joints and is assigned the same number as the associated joint. The purpose of the terminating link is 
to allocate a item in the data structure to store all of the information concerning the associated joints, for 
example, their position and the viscous friction coefficients. The D-H kinematic parameters for the terminating 
links are chosen so that the loop closure equations can be written in terms of homogeneous transforms. This 
will be described in more detail in the section concerning the constraint equations. 

If link i is a successor link then the associated joint is either translational or rotational. In either case the 
joint position is denoted by g*. If the joint is translational then qi = d, . If the joint is rotational then qi = 0,-. 

Finally, connector links are numbered starting at m + r 4* 1 on up to the total number of links, both 
imaginary and real, contained in the system. 

The resulting data structure for the example system is illustrated in Figure 1(b). The convention we use 
is that the items in the data structure associated with connector links are connected to their predecessor with 
small solid circles and items associated with successor links are connected to their predecessor with small white 
circles. 


2*2 System Kinematics 

From Figure 1(b) one can see that the position of each link can be determined with respect to the inertial 
coordinates by starting at the inertial link and successively computing each link position while moving out 
from the inertial link. Let <j>i denote the number of the immediate predecessor of link i. If the homogeneous 
transform of link <f>i coordinates with respect to the inertial link 0 coordinates, Tq\ is known then the 
homogeneous transform of link i coordinates with respect to the inertial link is T* 0 and can be computing 
using the following equation. 

n = T$'T\ t 
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where 2^. is a function of the a,-, di , a*, Note that the same equation applies if link i is a connector link 
or a successor link. 

This paper uses the spatial notation by Featherstone, [3,5]. With this notation transformation matrices 
are 6x6 matrices. The spatial transformation matrix from link i to link <j>i is: 


X 


i 

<t>i 


< . o 
K(P*,)A+, A '+. 


where O is the 3 x 3 null matrix, A\ % and are the upper left 3x3 submatrix and upper right 3x1 
submatrix of the homogeneous transformation matrix T^., respectively. The 3x3 matrix Jif() is a skew 
symmetric matrix such that K(a)b = a x b for any 3x1 vectors a and b. 

As with homogeneous transforms, the spatial transformation from link i coordinates to link 0 coordinates 
can be computed given that of its immediate predecessor by multiplying the transforms together. 


4u 


x * 0 = xpx^ ( 1 ) 

The spatial velocity and acceleration of link i can be determined given those of its immediate predecessor, 


f v^ i + Siq { if link i is a successor 
\ if link i is a connector 

f -f -f x Si4i if link i is a successor 
\ v # . if link i is a connector 


( 2 ) 

(3) 


where a* is the third column of Xp if joint i is rotational or the sixth column of Xp if joint t is translational. 

Thus, one starts at the root of the binary tree data structure and works out along the branches using the 
above equations to successively compute the spatial transformation matrix, velocity and the acceleration of 
each link. 


2.3 Kinematic Constraint Equations 

In general, the system is graph structured with n degrees of freedom. For this reason, we partition the set 
of joints into two mutually exclusive sets. There are n joints in the first set. They are called primary joints 
since their positions are independent of any other joint positions in the system. These joint positions are 
combined into a single n x 1 vector called, Q . The joints in the second set are called secondary joints, as 
their positions are strictly functions of the primary joint positions. Thus, the positions of all the joints in 
the system are functions of the primary joint positions. We can write this fact in the form of the following 
constraint equation. 

q = U(Q) (4) 

Note that for each Qi there is a q, such that Q, = qj. Given U(Q), Q, and Q, the velocity and acceleration 

of all the joints can be determined. 

q = E(Q)Q (5) 

q = E{Q)Q + E(Q)Q (6) 


where 


E(Q) = 


mo) 

dQ 


(7) 


For a given system these equations are usually fairly simple. Often the matrix £(Q) is constant. For example, 
it is simply the identity matrix for a tree structured system. However, for some systems these equations can 
become complex. For these systems we have found that the e-algebra is very convenient for evaluating the 
time derivatives of the joint positions [9]. Using this algebra one only has to program to solution to Equation 
4, change the order of the algebra and automatically obtain the solution to equations 5 and 6. 


136 



The equations of constraint are obtained from the loop closure equations. For the system shown in Figure 
1(a) there are two independent loops. The two loop closure equations are: 

Tj( 9l )rl 3 = Tj°T} It 4 u ( g4 )2i(f 5 )Tf (? 8 ) (8) 

and 

T' 0 ( qi )Tl(q,)T\ 2 = Tl°T 6 10 ( q6 )Tl(q 7 )T 9 7 (q 9 ) (9) 

These two matrix equations consist of a total of 32 scalar equations of which only six are independent. Thus, 
we can determine q\, g 2j 94 > 96 > 9&> and 99 as a function of the joint positions q$ and qj. Using the above 
notation, g 5 = Q Xt q 7 = Q 2 and q 3 = Q 3 . These three equations along with the six obtained from equations 8 
and 9 give us the function U(Q) defined above. 

2.4 System Dynamics 

The equations of motion of a system can be written in the following form: 

r = H(Q)Q + C(Q, Q)Q + G e (Q) 

where 

r = n x 1 vector of active forces 

R(Q) — n x n pseudo moment of inertia matrix 

C(Q,Q)Q = nxl vector of friction, centrifugal and Coriolis terms 

G«(Q) = nxl vector of external force components 

In the simulation program the state variables are the position and velocity of the primary variables, Q. To 
simulate the system one determines the acceleration of these variables from the above equation: 

Q = H{Q)-\t - C(Q, Q)Q - G'{Q)) 

We use the first method presented in [10] to compute the H(Q) matrix. Although this method was originally 
was presented for serial link manipulators, the approach is still applicable if one uses an inverse dynamics 
algorithm for systems with closed kinematic loops [6,7,8]. 

The inverse dynamics algorithm we use is a three step process [8]. 

1. Given the desired positions, velocities, and accelerations of the independent variables, Q, calculate the 
corresponding positions, velocities, and accelerations of the joints, q , using equations, 4 through 6. 

2. For each link, determine pj, the sum of the friction force and the projection of the sum of the inertial 
forces on the axis of motion of joint j. 

3. Determine the active forces, r using the constraint equations and the pj computed in step 2. 

r = E(Q) T p 

where r is an n x 1 vector of the Tj and p is an (m + r) x 1 vector of the pj . 

Step 2 is exactly the same procedure which would be used if the system was a tree structure system. In 
this case the pj would be identical to the active forces. The only difference is in the first step, wherein joint 
positions, velocities, and accelerations are determined which are consistent with the constraint equations, and 
the last step, wherein the constraint equations are again used in determining the active forces. 

Step 2 can be implemented in three steps. First, the position, velocity and acceleration of each link is 
computed using equations 1 through 3. This is done by starting at the inertial link and working out along the 
branches of the tree. Next, for each link j we determine fj , the sum of the external forces and inertial forces 
of link j and all of its descendants. This is done by starting at the tips of the branches of the tree and working 
back toward the inertial link of the tree using the following equations. Let Fj denote the inertial force of link 
j. Then: 

Fj = Mj = IjVj + Vj x IjVj (10) 
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Figure 3: Illustration of distance between two convex polytopes 


where Mj = Ij v j i® the momentum vector for the j — th link and Ij is the spatial moment of inertia 

matrix referred to inertial coordinates: 

h = xUjX\ V 

In this equation the matrix Lj i® the spatial moment of inertia matrix referred in link j coordinates: 





mjl 

mjKiZj) 


where mj is the mass of link j, is the location of the center of mass, and J_j is the moment of inertia matrix. 
Both £. and 1, are defined with respect to link j coordinates. The 3x3 skew symmetric matrix K(p.) is a 

function of the vector £. such that for any vector o, K(jK)a = x a. 

For connector and terminating links the Fj are zero since they have no mass. Let j be the index of the 
current link, and let it and 1 be the index of the its successor and connector links. The vector fj is used to 
denote the sum of the inertial and external forces for link j and all of it’s descendants. 


fj = Fj + F) + f k + f, (11) 

The last part of step 2 is to determine pj , the force due to friction, r\j, plus the projection of fj onto the 
axis of motion of joint j. 

Pj=»jfj+Vj ( 12 ) 

where i jj — djqj is the component of joint force due to viscous friction. 


2.5 Contact Dynamics 

To simulate contact we need a geometric model of the links in the system and a method of computing the 
distance between these objects. We use unions of convex polytopes to model the geometry of each link. An 
example of two objects is illustrated in figure 3. Polytopes were used as a model since the shape of links can 
be closely approximated by using a suitable number of vertices in the polytopes and a very efficient distance 
algorithm exist for computing the distance between convex polytopes, [4]. Given the position of the two 
polytopes the algorithm returns the distance, d , between them as well as the location of the point in each 
polytope which is nearest the other polytope, p a and p b . In addition, the unit line vector, tj, > 8 returned which 
passes through the near points of the two objects. 
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The distance between objects is calculated after each integration step in the simulation. The integration 
algorithm is a variable step size algorithm. When collision has not occurred, the integration step size is limited 
to ensure that each joint position changes by no more than a specified amount. As objects near each other, 
the step size is also limited so the objects will just touch at the next integration step. We say that two objects 
are just touching when the distance between them is equal to a small constant, c. An estimate of the time 
when the objects will collide is given by the following equation. 

collision time = t — 
d 

If the collision time is greater than zero, the integration step size is selected to be less than or equal to this 
value. If the collision time is negative, then the objects are moving apart from each other and this collision 
time is ignored. 

The value of d is easily computed from the unit line vector, ty, returned from the distance algorithm and 
the spatial velocities of the two objects, v Q and v*, which are obtained from the simulation algorithm. 

d = ij'(v a - v t ) 

The dynamics of contact is modeled like a spring. When the distance between the objects is less than e a 
force is applied to object A in the direction of ry with magnitude, / c , given by: 

f e = K P C -^- - K d d 

where K p and Kd are positive constants. Thus, the magnitude approaches infinity as the actual distance, d, 
approaches zero. The objective of this is to ensure that the objects never intersect. The derivative term is 
added to provided damping. The spatial force, f e exerted on object A is given by: 


fe = fcV 

Thus, we are modeling hard contacts with no sliding friction. The spatial force exerted on object B is the 
negative of this value. These forces are included into the dynamic simulation as external forces exerted on the 
links as described in the above section on dynamics. 

3 Control 

Feedback signals used be the controllers come in two forms: spatial feedback vectors, Fj , which are associated 
with the links of the manipulator and scalar feedback functions, , which are associated with the joints 
of the manipulator. These can be any functions of the desired trajectory and the actual trajectory of the 
manipulator. The method used to calculate the required actuator forces is as follows. 

1 . The feedback control law is implemented through the use of a set of spatial feedback vectors, Fj y which 
are defined for each link in the system, and scalar feedback functions, 17^ , which are defined for each 
joint of the manipulator. 

2 . The Tj are computed using the Fj and rjj as in the following recursive equations: 

/; = Fj +/*+/» 

h = tj'fj+Vj 
r = E(Q) T p 

where k and l are the immediate descendants of link j, p is an (m + r) x 1 vector of the Pj. 

These types of calculations are common to a variety of controller algorithms and are included only for the 
users convenience. The choice of the Fj and fjj dictates the character of the controller being implemented. 
The following example is for an inverse dynamics controller. 


139 



Figure 4: Example System Used in Simulation 

In the inverse dynamics controller, both the spatial feedback vectors and feedback functions are used. The 
following variables are used to compute them. 


Q P = Q*-Q 

Q = Q d + K d Q t + K p Q t 
q = E(Q)Q 


$ = E(Q)Q + E(Q)Q 

Vq = 0 

vo = 0 


where K d and K p are positive constants. 

For i > 0, the spatial acceleration vectors are computed using recursive equations: 


is a successor 
is a connector 


f if link i 1 

Vi “ { v+t if link i i 

I _ | + Siit + Bi<ii if link i ls 

V * “ \ if link i is 

The spatial feedback vectors are functions of these vectors. For i > 0: 

* _ f Iii>i + Vi x IiVi if link i is 
* \ 0 if link i is 

The scalar feedback functions are: 


a successor 
a connector 


a successor 

a connector or terminator 


if link i is a successor 
if link i is a connector 


The hat over the d, and the indicates they are only estimates of the corresponding components of the actual 
system. Thus, Ii is the spatial moment of inertia matrix of link i using the estimated values of the link inertial 
parameters. 

This control law is a recursive implementation of the following function: 


= H(Q)Q + C(Q, Q)Q 
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4 Simulation Results 


A simulation was performed of a two armed robot consisting of two PUMA 560 manipulators mounted on 
a single base. The objective of the simulation is to test the ability of the manipulators to grasp a floating 
object using the above inverse dynamics control algorithm. Each manipulator is equipped with an end-effector 
having three rigid fingers. There are three distinct phases of this task: approach, contact, and coordination. 
The approach phase is when both manipulators are approaching the object, but nether has contacted the 
object. The contact phase is the time after the first contact is made by either manipulator and before the 
coordination phase. The coordination phase is after both manipulators have made three point contacts with 
the object. A relatively low position feedback gain at each joint of the manipulators to provide the positional 
compliance required during contact with the object. 

The system is illustrated in figure 4. To model the floating object, five massless links were inserted between 
the base coordinate frame and the object coordinates. These are denoted by the five large dotted line circles 
in the figure. 

As expected the controller performed fairly well during the approach phase. Because of the small gains 
the settling time was about one second. This could be improved by increasing the gains with the result of 
reducing the compliance during the contact and coordination phases. The controller did not perform as well 
during the contact and coordination phase. Significant position errors and forces were observed due to the 
interaction of the object with the manipulator. 

5 Conclusion 

This paper presented an efficient method of simulating the motion of space based robots. The simulation 
includes not only the dynamics of the manipulators but also the dynamics of the objects in which the ma- 
nipulators come into contact. Thus, the effect the robots have on the world in which they are a part is an 
important aspect of the simulation. 

An example simulation was provided of a two armed robot grappling a satellite. It was found that the 
inverse dynamics controller worked reasonably well during the approach phase of this task, but it did not work 
very good during the contact and coordination phases. The controller was not successful in obtaining a stable 
grappling of the object. The conclusion is that a different control law is required for the three distinct phases 
of this task: approach, contact, and coordination. A position control should be used during the approach 
phase. A compliant motion control should be used during the contact phase and a dual-arm coordinated 
motion control should be used to manipulate the satellite. 

In our simulation and the real robot controller we have the capability of switching between any one of 
a number a control laws from one sample period to the next. The problem we face is in deciding when a 
new control law should used. This is a complex decision in which several factors must be considered. The 
decisions for the switching times and the selection of the control laws should be based upon the task definition 
and available sensory measurements, such as tactile and force sensors. Except in a very simple manner, we 
currently have no way of either simulating this decision process or implementing it in the real robot controller. 
We believe the complexity dictates the use of artificial intelligence techniques such as expert systems to make 
these decisions. The lack of this decision making component is the greatest limitation of our system. 

Finally, the last phase of controller development is the installation and testing with the real robot. This 
process is facilitated in the our laboratory by programming both the simulation and the real robot controller in 
the same language, Ada. Also, the definition of the spatial and scalar feedback functions are incorporated into 
the simulation as well as the real controller with identical pieces of Ada software. Thus, once the simulation 
results are acceptable, the controller code from the simulation is transfered to the robot’s control computer, 
where it is recompiled and linked into the robot controller software. Future goals will be to integrate an expert 
system into both the simulation and the real robot motion controller to handle the control law switching 
decisions. 
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